/*****************************************************************************
*  Copyright Statement:
*  --------------------
*  This software is protected by Copyright and the information contained
*  herein is confidential. The software may not be copied and the information
*  contained herein may not be used or disclosed except with the written
*  permission of MediaTek Inc. (C) 2007
*
*****************************************************************************/

#include "StdAfx.h"
#include "SerialAPI.h"
#include "ConstDef.h"

//Max number of Message Mapping can be registered
static const int MAX_MAPPING_COUNT = 16;
//Max Nema Log String Length
#define MAX_STR_LEN 30000

char g_NMEARemain[2048];
char g_NMEAbuf[2048];   
char TempBuf[2048];
GPSInfo g_gpsInfo;
SVInfo  g_svInfo[NUM_CH];
ChInfo  g_chInfo[PSEUDO_CH];
Version g_FW;
HANDLE g_hCom;        
HANDLE g_hCommThread;
FILE *g_fNMEALog;
int  g_Ack;
int  g_Fail_Ack;
bool g_fgSVUpdate;
bool g_fgChUpdate;
int  g_TimerInterval;
long g_RTCM_Baudrate;
char g_NMEASetting[75];
wstring g_strNMEALog;
wstring g_strFirmwareVer;

BOOL ReadBusy;
MsgMapping MsgMap[MAX_MAPPING_COUNT];
int nNextMapIdx = 0;

void FixSVMapping(void);
bool FetchField(char *start, char *result);
void NMEA_Parse(void);

BOOL RegisterMsg(HWND hWndView, UINT nMsg)
{
    if ( nNextMapIdx == MAX_MAPPING_COUNT )
	{
	   return FALSE;
	}
	else
	{
	   MsgMap[nNextMapIdx].hWndReg = hWndView;
	   MsgMap[nNextMapIdx].MsgReg = nMsg;
	   nNextMapIdx ++;
	   return TRUE;
	}
}

void SendMsgToRegWnd(UINT nMsg, WPARAM wParam, LPARAM lParam )
{
   int i;
   for (i=0; i<MAX_MAPPING_COUNT; i++)
   {
	   if ( MsgMap[i].MsgReg == nMsg )
	   {
		   ::SendMessage(MsgMap[i].hWndReg, nMsg, wParam, lParam); 
	   }
   }
}

BOOL MByteToWChar(LPCSTR lpcszStr, LPWSTR lpwszStr, DWORD dwSize)
{
   // Get the required size of the buffer that receives the Unicode 
   // string. 
   DWORD dwMinSize;
   dwMinSize = MultiByteToWideChar (CP_ACP, 0, lpcszStr, -1, NULL, 0);
  
   if(dwSize < dwMinSize)
   {
     return FALSE;
   }    
   // Convert headers from ASCII to Unicode.
   MultiByteToWideChar (CP_ACP, 0, lpcszStr, -1, lpwszStr, dwMinSize);  
   return TRUE;
}

BOOL WCharToMByte(LPCWSTR lpcwszStr, LPSTR lpszStr, DWORD dwSize)
{
   DWORD dwMinSize;
   dwMinSize = WideCharToMultiByte(CP_OEMCP,NULL,lpcwszStr,-1,NULL,0,NULL,FALSE);
   if(dwSize < dwMinSize)
   {
    return FALSE;
   }
   WideCharToMultiByte(CP_OEMCP,NULL,lpcwszStr,-1,lpszStr,dwSize,NULL,FALSE);
   return TRUE;
}

void InitialGPSData(void)
{
	memset( &g_gpsInfo, NULL, sizeof(GPSInfo));
	memset( &g_svInfo, NULL, sizeof(g_svInfo));
	memset( &g_FW, NULL, sizeof(Version));
	memset( &g_chInfo, NULL, sizeof(g_chInfo));
	memset( g_NMEASetting, NULL, sizeof(g_NMEASetting));
	memset( g_NMEAbuf, NULL, sizeof(g_NMEAbuf));
	memset( g_NMEARemain, NULL, sizeof(g_NMEARemain));

	g_hCom=NULL;
	g_hCommThread=NULL;
	g_fNMEALog=NULL;
	g_fgChUpdate=false;
	g_Ack=0;
	g_Fail_Ack = 0;
	g_fgSVUpdate=false;
	g_TimerInterval=1000;
	g_RTCM_Baudrate=0;
}

void NMEA_Combine(char *buf)
{
   char *GP, *lastGP, *ptr;

   // avoid overflowing while no NMEA(GP) sentences output
   if(strlen(g_NMEARemain) > sizeof(g_NMEARemain) * 0.75)
      memset(g_NMEARemain, NULL, sizeof(g_NMEARemain));

   memset(g_NMEAbuf,NULL,sizeof(g_NMEAbuf));
   strcpy(g_NMEAbuf, g_NMEARemain);
   strcat(g_NMEAbuf, buf);

   //initialize
   GP = lastGP = NULL;
   ptr = g_NMEAbuf;

   // there must be GP sentences, couldn't change to PMTK setences
   // because there aren't always PMTK sentences.
   while((GP=strstr( ptr, "$GP")) != NULL)
   {
      lastGP = GP;
      ptr = GP+1;
   }

   while((GP=strstr( ptr, "$PMTK")) != NULL)
   {
      lastGP = GP;
      ptr = GP+1;
   }

   if(lastGP != NULL)
   {
      strcpy(g_NMEARemain, lastGP);
      *(lastGP) = '\0';
   }
   // lastGP == NULL means NMEAbuf doesn't contain any NMEA sentence
}
void NEMALogShowPreProcess()   //Nemalog 
{ 
	char NMEA[256];
	TCHAR strTemp[256];
	memset(strTemp,0,sizeof(TCHAR)*256);
	char *start=NULL, *end=NULL;
	wstring str;  
 	start = strstr(g_NMEAbuf, "$GP");
	if(start != NULL)
		end = strstr(start, "\r\n");

	while((start != NULL) && (end != NULL))
	{	
		memset(NMEA,0,sizeof(char)*256);
		strncpy( NMEA, start, end-start);
		NMEA[end-start]='\0';
		
		strcat(NMEA,"\r\n");
		mbstowcs(strTemp,NMEA,256);
		str=strTemp;
		
		if ( (g_strNMEALog.length() + str.length()) - MAX_STR_LEN > 0 )
		{
			g_strNMEALog = L"";
		}
			
		g_strNMEALog+= str;


	  start = strstr(end, "$GP");
      if(start != NULL)
	  {
         end = strstr(start, "\r\n");
	  }
   }
}

BOOL StartLogNema(const char* fpath)
{
	if(g_hCommThread == NULL || g_fNMEALog != NULL)
	{
		return FALSE;
	}

    g_fNMEALog = fopen(fpath, "a+");

	if (g_fNMEALog == NULL)
	{
		return FALSE;
	}
	else
	{
		return TRUE;
	}
}

void StopLogNema()
{
	if(g_fNMEALog)
	{
		fclose(g_fNMEALog);
		g_fNMEALog = NULL;
	}
}


#define READ_BUF_SIZE 256

DWORD WINAPI SerialThreadProcess(void)
{
	char buf[READ_BUF_SIZE + 1];
	DWORD NumBytes=0; 
	while(1)
	{   
		ReadBusy=true;
		//receive nema data
		ReadFile(g_hCom, buf, READ_BUF_SIZE, &NumBytes, NULL);
		ReadBusy=false;

		if(NumBytes > 0) 
		{
			buf[NumBytes] = '\0';

			// Log to file
           if(g_fNMEALog != NULL)
			{
				fputs( buf, g_fNMEALog);
			}

			// NMEA combine
			NMEA_Combine(buf);
			//create Show Nema log thread
			NEMALogShowPreProcess();          //data not lost!
            // NMEA parsing
			NMEA_Parse();		
			FixSVMapping();
			SendMsgToRegWnd(GPS_STATUS_UPDATE, 0, 0);
		}
	}
	return(0);
}

void CreateComRecThread(void)
{
	DWORD threadID;
	g_hCommThread = ::CreateThread((LPSECURITY_ATTRIBUTES)NULL, 1024,(LPTHREAD_START_ROUTINE)SerialThreadProcess,NULL, 0, &threadID);
	if (g_hCommThread == NULL)
	{
	//	::AfxMessageBox(_T("Create RS232 Thread Fail!"));
		::PostQuitMessage(0);
	}
	else
	{
	    //SF: Set WinCE Priority to Realtime below drivers
		SetThreadPriority(g_hCommThread, THREAD_PRIORITY_TIME_CRITICAL);
	}
}

void TerminateComRecThread(void)
{	
	TerminateThread(g_hCommThread,0);          
	CloseHandle(g_hCommThread);          //realse the resource
	g_hCommThread=NULL;
}

BOOL OpenSerialPort(wstring ComPort,wstring Baudrate)
{
    DCB dcb;
	BOOL fSuccess;

	g_hCom=CreateFile(ComPort.c_str(), GENERIC_READ|GENERIC_WRITE, 0,NULL , OPEN_EXISTING, 0, NULL);

	if (INVALID_HANDLE_VALUE == g_hCom)
	{
//		MessageBox(NULL,L"Open Com err",L"Open Com err",MB_YESNO);
		return false;
	}

	fSuccess = GetCommState(g_hCom, &dcb);		

	if (fSuccess)
	{
		dcb.BaudRate=_ttol(Baudrate.c_str());
		dcb.ByteSize = 8;
		dcb.Parity = NOPARITY;
		dcb.StopBits = ONESTOPBIT;
		dcb.fOutxDsrFlow = FALSE;
		dcb.fOutxCtsFlow = FALSE;
		dcb.fDtrControl = DTR_CONTROL_DISABLE;
		dcb.fRtsControl = RTS_CONTROL_ENABLE;
		dcb.fInX = FALSE;				// No Xon/Xoff flow control
		dcb.fOutX = FALSE;
		dcb.fBinary = TRUE;
		dcb.fAbortOnError = FALSE;		// Do not abort reads/writes on error
		dcb.fErrorChar = FALSE;			// Disable error replacement
		dcb.fNull = FALSE;				// Disable null stripping

		fSuccess = SetCommState(g_hCom, &dcb);
		if (fSuccess)
		{
			COMMTIMEOUTS timeouts;

			//SetupComm(hUARTHandle, rxbufsize, txbufsize);
			GetCommTimeouts(g_hCom, &timeouts);
			timeouts.ReadIntervalTimeout = 0;
			timeouts.ReadTotalTimeoutConstant = 100;
			timeouts.ReadTotalTimeoutMultiplier = 0;
			timeouts.WriteTotalTimeoutConstant = 0;
			timeouts.WriteTotalTimeoutMultiplier = 0;
			SetCommTimeouts(g_hCom, &timeouts);
		}
	}
	else
	{

		if(INVALID_HANDLE_VALUE != g_hCom)
		{
			CloseHandle(g_hCom);
			g_hCom = NULL;
			return false;
		}
	}
	return true;
}

void CloseSerialPort()
{
    CloseHandle(g_hCommThread);      //realse the com watch thread
	g_hCommThread=NULL;

	if(g_hCom)                      //realse com handle                          
    {
      CloseHandle(g_hCom);
      g_hCom = NULL;
    }
}

BOOL WriteCmd(const char *Cmd)          //NO '\0'
{
	DWORD dwWrite=0;
	int i;
	i=0;
	if(g_hCom)
	{
		while(ReadBusy)
		{
			i++;
			Sleep(2);
			if(i>1000)
			{
				TerminateComRecThread();
				goto Next;
			}
		}
		SuspendThread(g_hCommThread);
Next:
		WriteFile( g_hCom, Cmd, strlen(Cmd), &dwWrite, NULL);
		if(dwWrite != strlen(Cmd))
		{
		//	AfxMessageBox(_T("Command Send Fail!"));
		}

		// Sleep 1000 ms in order to avoid receiving NMEA sentence which reboot before
		Sleep(500);
		if(i>1000)
			CreateComRecThread(); 
		else
			ResumeThread(g_hCommThread);
		return true;
	}
	else
	{
	//	AfxMessageBox(_T("Com Port No Open!"));
		return false;
	}
}

//the Sentence availability-----------------------------------------------------------------------
unsigned char CheckSum(char *buf, int size)
{
   int i;
   char chksum=0, chksum2=0;

   if(size < 5)
      return false;

   chksum = buf[1];
   for(i = 2; i < (size - 2); i++)
   {
      if(buf[i] != '*')
      {
        chksum ^= buf[i];
      }
      else
      {
        if(buf[i + 1] >= 'A')
        {
          chksum2 = (buf[i+1]-'A'+10)<<4;
        }
        else
        {
          chksum2 = (buf[i+1]-'0')<<4;
        }

        if(buf[i + 2] >= 'A')
        {
          chksum2 += buf[i+2]-'A'+10;
        }
        else
        {
          chksum2 += buf[i+2]-'0';
        }
        break;
      }
    }

   /* if not found character '*' */
   if(i == (size - 2))
   {
      return (false);
   }

   if(chksum == chksum2)
   {
     return (true);
   }
   else
   {
     return (false);
   }
}
//---------------------------------------------------------------------------
bool FetchField(char *start, char *result)
{
   char *end;

   if(start == NULL)
      return false;

   end = strstr( start, ",");
   // the end of sentence
   if(end == NULL)
      end = strstr(start, "*");

   if(end-start>0)
   {
     strncpy( result, start, end-start);
     result[end-start]='\0';
   }
   else   // no data
   {
     result[0]='\0';
     return false;
   }

   return true;
}
//NEMA FORMAT PARSE
//---------------------------------------------------------------------------
void Channel_Parse( char *head)
{
   char *start, result[20], tmp[20];

   // check checksum
   if(CheckSum(head, strlen(head)))
   {
      start = strstr( head, ",");
      if(start != NULL)
         start = start +1;
      else
         return;

      for(int i=0; i<PSEUDO_CH ; i++)
      {
         if(!FetchField( start, result))
            return;

         // SVid
         strncpy( tmp, result, 2);
         tmp[2]='\0';
         g_chInfo[i].SVid = atoi(tmp);

         // SNR
         strncpy( tmp, result+2, 2);
         tmp[2]='\0';
         g_chInfo[i].SNR = atoi(tmp);

         // Status
         if(result[4]=='0')
            g_chInfo[i].Status = 0;
         else if(result[4]=='1')
            g_chInfo[i].Status = 1;
         else
            g_chInfo[i].Status = 2;

         start = strstr( start, ",");
         if(start != NULL)
            start = start +1;
         else                // find '*'
            break;
      }
      g_fgChUpdate = true;
   }
}

void Ack_Parse(char *head)
{
	int CmdAck=0, type=0;
	char *start, result[20];
	TCHAR strTemp[256];
	wstring str;
	// check checksum
	if(CheckSum(head, strlen(head)))
	{
		if(strncmp(head, "$PMTK001", 8)==0)
		{
			start = strstr( head, ",")+1;
			FetchField( start, result);
			CmdAck = atoi(result);
			start = strstr( start, ",")+1;
			FetchField( start, result);
			type = atoi(result);
			// Success
			if(type==3)
			{
				g_Ack = CmdAck;
			}
			else if(type ==2)
			{
				g_Fail_Ack = CmdAck;
			}
			
		}
		else if(strncmp(head, "$PMTK500", 8)==0)
		{
			int Fix_Int=0;
			int FixRate=0;
			start = strstr(head, ",")+1;
			FetchField( start, result);
			Fix_Int = atoi(result);
			if(Fix_Int <= 1000)
            FixRate = 1000/Fix_Int;
			memset(strTemp,0,sizeof(TCHAR)*256);
			swprintf(strTemp,L"%d",FixRate);
			str = strTemp;
		//	str.Format(_T("%d"),FixRate);	
		}
		else if(strncmp(head, "$PMTK501", 8)==0)
		{
			int DGPSMode=0;
			start = strstr(head, ",")+1;
			FetchField( start, result);
			DGPSMode = atoi(result);
		}
		else if(strncmp(head, "$PMTK514", 8)==0)
		{
			start = strstr(head, ",")+1;
			strcpy(g_NMEASetting, start);
			
		}
		else if(strncmp(head, "$PMTK519", 8)==0)
		{
			int IntegrityMode=0;
			start = strstr(head, ",")+1;
			FetchField( start, result);
			IntegrityMode = atoi(result);		
		}
		else if(strncmp(head, "$PMTK530", 8)==0)
		{
			int Datum_index;
			start = strstr(head, ",")+1;
			FetchField( start, result);
			Datum_index = atoi(result);
		}
		else if(strncmp(head, "$PMTK556", 8)==0)
		{
			float TCXO=1000;
			start = strstr(head, ",")+1;
			start = strstr(start, ",")+1;
			FetchField( start, result);
			TCXO = float(atof(result) * 1e6);
		}
		else if(strncmp(head, "$PMTK590", 8)==0)
		{
			int flash_times=0;
			start = strstr(head, ",")+1;
			FetchField( start, result);
			memset(strTemp,0,sizeof(TCHAR)*256);
			mbstowcs(strTemp,result,128);
			str=strTemp;
		}
		else if(strncmp(head, "$PMTK702", 8)==0)
		{
			if(strncmp(head, "$PMTK702,1,0,", 13)==0)
			{
				start = strstr(head, ",")+1;
				start = strstr(start, ",")+1;
				start = strstr(start, ",")+1;
				FetchField( start, result);
				g_RTCM_Baudrate=atol(result);
				//RTCM_Update
				memset(strTemp,0,sizeof(TCHAR)*256);
				mbstowcs(strTemp,result,128);
				str=strTemp;
			}
		    else
			{
				g_RTCM_Baudrate = 0;
			}
		}
		else if(strncmp(head, "$PMTK705", 8)==0)
		{
			// MCore Version
			start = strstr( head, ",")+1;
			FetchField( start, g_FW.MCore);
			// Customer Version
			start = strstr( start, ",")+1;
			FetchField( start, g_FW.Customer);
		
			memset(strTemp,0,sizeof(TCHAR)*256);
			TCHAR strTemp1[256],strTemp2[256];
			memset(strTemp1,0,sizeof(TCHAR)*256);
			memset(strTemp2,0,sizeof(TCHAR)*256);
			mbstowcs(strTemp1,g_FW.MCore,256);	
			mbstowcs(strTemp2,g_FW.Customer,256);
			swprintf(strTemp,L"%s %s",strTemp1,strTemp2);
			g_strFirmwareVer = strTemp;
           SendMsgToRegWnd(GPS_FIRMWARE_VERSION, 0, 0);
		}
	}
}
//---------------------------------------------------------------------------
void GLL_Parse( char *head)
{
   // $GPGLL,2446.367638,N,12101.356226,E,144437.000,A,A*56
   char *start, result[20], tmp[20], *point;
   int len=0;
   char FixService;

   // check checksum
   if(CheckSum(head, strlen(head)))
   {
      // Position(Lat)
      start = strstr( head, ",");
      if(start != NULL)
         start = start +1;
      else
         return;
      if(FetchField(start, result))
      {
         point = strstr( result, ".");
         len = (point-2)-result;
         strncpy(tmp, result, len);
         tmp[len]='\0';
         g_gpsInfo.Lat = float(atof(tmp));
         strncpy(tmp, result+len, strlen(result)-len);
         tmp[strlen(result)-len]='\0';
         g_gpsInfo.Lat += float(atof(tmp)/60.0);
      }

      // N or S
      start = strstr( start, ",");
      if(start != NULL)
         start = start +1;
      else
         return;
      if(FetchField(start, result))
      {
         if(*result=='S')
            g_gpsInfo.Lat = -g_gpsInfo.Lat;
      }

      // Position(Lon)
      start = strstr( start, ",");
      if(start != NULL)
         start = start +1;
      else
         return;
      if(FetchField(start, result))
      {
         point = strstr( result, ".");
         len = (point-2)-result;
         strncpy(tmp, result, len);
         tmp[len]='\0';
         g_gpsInfo.Lon = float(atof(tmp));
         strncpy(tmp, result+len, strlen(result)-len);
         tmp[strlen(result)-len]='\0';
         g_gpsInfo.Lon += float(atof(tmp)/60.0);
      }

      // E or W
      start = strstr( start, ",");
      if(start != NULL)
         start = start +1;
      else
         return;
      if(FetchField( start, result))
      {
         if(*result=='W')
            g_gpsInfo.Lon = -g_gpsInfo.Lon;
      }

      // UTC Time
      start = strstr( start, ",");
      if(start != NULL)
         start = start +1;
      else
         return;
      if(FetchField( start, result))
      {
         // Hour
         strncpy( tmp, result, 2);
         tmp[2]='\0';
         g_gpsInfo.hour = atoi(tmp);
         // Min
         strncpy( tmp, result+2, 2);
         tmp[2]='\0';
         g_gpsInfo.min = atoi(tmp);
         // Sec
         strncpy( tmp, result+4, strlen(result)-4);
         tmp[strlen(result)-4]='\0';
         g_gpsInfo.sec = float(atof(tmp));
      }   

      // The positioning system Mode Indicator and Status fields shall not be null fields.
      // Data valid
      start = strstr( start, ",");
      if(start != NULL)
         start = start +1;
      else
         return;

      if(!FetchField( start, result))
         return;
      
      if(*result=='A')
      {
         // Fix Type
         if(g_gpsInfo.FixType == 0)
            g_gpsInfo.FixType = 1;   // Assume 2D, if there's no other info.

         // Fix Service
         start = strstr( start, ",");
         if(start != NULL)
             start = start +1;
         else
            return;

         if(!FetchField( start, result))
            return;

         FixService = *result;

         switch(FixService)
         {
            case 'A':
            {
               g_gpsInfo.FixService = 1;
               break;
            }
            case 'D':
            {
               g_gpsInfo.FixService = 2;
               break;
            }
            case 'E':
            {
               g_gpsInfo.FixService = 6;
               break;
            }
         }
      }
      else // Data invalid
      {
         g_gpsInfo.FixType = 0;    // NoFix
         g_gpsInfo.FixService = 0; // NoFix
      }
   }
}
//---------------------------------------------------------------------------
void RMC_Parse( char *head)
{
   // $GPRMC,073446.000,A,2446.3752,N,12101.3708,E,0.002,22.08,121006,,,A*6C

   char *start, result[20], tmp[20], *point;
   int len=0;

   // check checksum
   if(CheckSum(head, strlen(head)))
   {
      // UTC time : 161229.487
      start = strstr( head, ",");
      if(start != NULL)
         start = start +1;
      else
         return;
      if(FetchField( start, tmp))
      {
         // Hour
         strncpy( result, tmp, 2);
         result[2]='\0';
         g_gpsInfo.hour = atoi(result);
         // Min
         strncpy( result, tmp+2, 2);
         result[2]='\0';
         g_gpsInfo.min = atoi(result);
         // Sec
         strncpy( result, tmp+4, strlen(tmp)-4);
         result[strlen(tmp)-4]='\0';
         g_gpsInfo.sec = float(atof(result));
      }

      // valid
      start = strstr( start, ",");
      if(start != NULL)
         start = start +1;
      else
         return;
      if(!FetchField( start, result))
         return;

      if(*result == 'A')
      {
         if(g_gpsInfo.FixType == 0)
            g_gpsInfo.FixType = 1;      // Assume 2D

         if(g_gpsInfo.FixService == 0)
            g_gpsInfo.FixService = 1;   // Assume SPS
      }
      else
      {
         g_gpsInfo.FixType = 0;    // NoFix
         g_gpsInfo.FixService = 0; // NoFix
      }

      // Position(Lat) : 3723.2475(N)
      start = strstr( start, ",");
      if(start != NULL)
         start = start +1;
      else
         return;
      if(FetchField( start, result))
      {
         point = strstr( result, ".");
         len = (point-2)-result;
         strncpy(tmp, result, len);
         tmp[len]='\0';
         g_gpsInfo.Lat = float(atoi(tmp));
         strncpy(tmp, result+len, strlen(result)-len);
         tmp[strlen(result)-len]='\0';
         g_gpsInfo.Lat += float(atof(tmp)/60.0);
      }
	  else  //Can not fetch Lat field
	  {
	     g_gpsInfo.Lat = 0;
	  }

      // N or S
      start = strstr( start, ",");
      if(start != NULL)
         start = start +1;
      else
         return;
      if(FetchField( start, result) && g_gpsInfo.Lat!=0)
      {
         if(*result=='S')
            g_gpsInfo.Lat = -g_gpsInfo.Lat;
      }

      // Position(Lon) : 12158.3416(W)
      start = strstr( start, ",");
      if(start != NULL)
         start = start +1;
      else
         return;
      if(FetchField( start, result))
      {
         point = strstr( result, ".");
         len = (point-2)-result;
         strncpy(tmp, result, len);
         tmp[len]='\0';
         g_gpsInfo.Lon = float(atoi(tmp));
         strncpy(tmp, result+len, strlen(result)-len);
         tmp[strlen(result)-len]='\0';
         g_gpsInfo.Lon += float(atof(tmp)/60.0);
      }
	  else  //Can not fetch Lat field
	  {
	     g_gpsInfo.Lon = 0;
	  }

      // E or W
      start = strstr( start, ",");
      if(start != NULL)
         start = start +1;
      else
         return;
      if(FetchField( start, result) && g_gpsInfo.Lat!=0)
      {
         if(*result=='W')
            g_gpsInfo.Lon = -g_gpsInfo.Lon;
      }

      // Speed : 0.13
      start = strstr( start, ",");
      if(start != NULL)
         start = start +1;
      else
         return;
      if(FetchField( start, result))
      {
         g_gpsInfo.Speed = float(atof(result) * Knot2Kmhr);
      }

      // Track : 309.62
      start = strstr( start, ",");
      if(start != NULL)
         start = start +1;
      else
         return;
      if(FetchField( start, result))
      {
         g_gpsInfo.Track = float(atof(result));
      }

      // Date : 120598
      start = strstr( start, ",");
      if(start != NULL)
         start = start +1;
      else
         return;
      if(FetchField( start, result))
      {
         // Day
         strncpy(tmp, result, 2);
         tmp[2]='\0';
         g_gpsInfo.day=atoi(tmp);

         // Month
         strncpy(tmp, result+2, 2);
         tmp[2]='\0';
         g_gpsInfo.mon=atoi(tmp);

         // Year
         strncpy(tmp, result+4, 2);
         tmp[2]='\0';
         g_gpsInfo.year=atoi(tmp)+2000;
      }

      // skip Magnetic variation
      start = strstr( start, ",");
      if(start != NULL)
         start = start +1;
      else
         return;
      start = strstr( start, ",");
      if(start != NULL)
         start = start +1;
      else
         return;

      // mode indicator
      start = strstr( start, ",");
      if(start != NULL)
         start = start +1;
      else
         return;

      if(!FetchField( start, result))
         return;

      if(g_gpsInfo.FixType > 0)
      {
         switch(result[0])
         {
            case 'A':
            {
               g_gpsInfo.FixService = 1;
               break;
            }
            case 'D':
            {
               g_gpsInfo.FixService = 2;
               break;
            }
            case 'E':
            {
               g_gpsInfo.FixService = 6;
               break;
            }
         }
      }
   }
}
//---------------------------------------------------------------------------
void VTG_Parse( char *head)
{
   //$GPVTG,159.16,T,,M,0.013,N,0.023,K,A*34
   char *start, result[20];
   char FixService;

   // check checksum
   if(CheckSum(head, strlen(head)))
   {
      // Track
      start = strstr( head, ",");
      if(start != NULL)
         start = start +1;
      else
         return;
      if(FetchField( start, result))
      {
         g_gpsInfo.Track = float(atof(result));
      }

      // ignore
      start = strstr( start, ",");     // T
      if(start != NULL)
         start = start +1;
      else
         return;

      start = strstr( start, ",");     // NULL
      if(start != NULL)
         start = start +1;
      else
         return;

      start = strstr( start, ",");     // M
      if(start != NULL)
         start = start +1;
      else
         return;

      // Speed
      start = strstr( start, ",");
      if(start != NULL)
         start = start +1;
      else
         return;
      if(FetchField( start, result))
      {
         g_gpsInfo.Speed = float(atof(result) * Knot2Kmhr);
      }

      // ignore
      start = strstr( start, ",");     // N
      if(start != NULL)
         start = start +1;
      else
         return;

      start = strstr( start, ",");     // 0.023
      if(start != NULL)
         start = start +1;
      else
         return;

      start = strstr( start, ",");     // K
      if(start != NULL)
         start = start +1;
      else
         return;

      // Fix Service
      start = strstr( start, ",");
      if(start != NULL)
         start = start +1;
      else
         return;

      if(!FetchField( start, result))
         return;
      FixService = *result;

      if(FixService != 'N')
      {
         if(g_gpsInfo.FixType == 0)
            g_gpsInfo.FixType = 1;  //Assume 2D

         switch(FixService)
         {
            case 'A':
            {
               g_gpsInfo.FixService = 1;
               break;
            }
            case 'D':
            {
               g_gpsInfo.FixService = 2;
               break;
            }
            case 'E':
            {
               g_gpsInfo.FixService = 6;
               break;
            }
         }
      }
      else // NoFix
      {
         g_gpsInfo.FixType = 0;    // NoFix
         g_gpsInfo.FixService = 0; // NoFix
      }
   }
}
//---------------------------------------------------------------------------
void GSA_Parse( char *head)
{
   // $GPGSA,A,3,03,19,27,23,13,16,15,11,07,,,,1.63,0.95,1.32*03
   char *start, result[20];
   int sv_cnt=0, i;

   if(CheckSum(head, strlen(head)))
   {
      //Fix SV
      memset(&g_gpsInfo.fixSV, NULL, sizeof(g_gpsInfo.fixSV));

      //Valid
      start = strstr( head, ",");
      if(start != NULL)
         start = start +1;
      else
         return;
      if(!FetchField( start, result))
         return;

      if((*result == 'A') || (*result == 'M'))
      {
         // Fix Type
         start = strstr( start, ",");
         if(start != NULL)
            start = start +1;
         else
            return;
         if(!FetchField( start, result))
            return;
         g_gpsInfo.FixType = atoi(result)-1;

         if(g_gpsInfo.FixType > 0)          // Fix
         {
            if(g_gpsInfo.FixService == 0)
               g_gpsInfo.FixService = 1;    //Assume SPS FixSerivce
         }
         else
         {
            g_gpsInfo.FixType = 0;    // NoFix
            g_gpsInfo.FixService = 0; // NoFix
         }
      }
      else
      {
         g_gpsInfo.FixType = 0;    // NoFix
         g_gpsInfo.FixService = 0; // NoFix
      }

      for(i=0 ; i<12 ; i++)
      {
         start = strstr( start, ",");
         if(start != NULL)
            start = start +1;
         else
            return;

         FetchField( start, result);

         if(strlen(result)>0)
            g_gpsInfo.fixSV[sv_cnt++] = atoi(result);
      }

      //PDOP
      start = strstr( start, ",");
      if(start != NULL)
         start = start +1;
      else
         return;
      if(!FetchField( start, result))
         return;
      g_gpsInfo.PDOP = float(atof(result));

      //HDOP
      start = strstr( start, ",");
      if(start != NULL)
         start = start +1;
      else
         return;
      if(!FetchField( start, result))
         return;
      g_gpsInfo.HDOP = float(atof(result));

      //VDOP
      start = strstr( start, ",");
      if(start != NULL)
         start = start +1;
      else
         return;
      if(!FetchField( start, result))
         return;
      g_gpsInfo.VDOP = float(atof(result));
   }
}
//---------------------------------------------------------------------------
void GSV_Parse( char *head)
{
   // $GPGSV,3,1,09,03,63,020,43,19,76,257,37,27,14,320,30,23,39,228,37*79
   // $GPGSV,3,2,09,13,38,274,38,16,31,058,37,15,16,055,34,11,16,192,32*76
   // $GPGSV,3,3,09,07,15,043,26*40

   char *start, result[20];
   int sv_cnt=0, base=0, i;

   // check checksum
   if(CheckSum(head, strlen(head)))
   {
      // ignore
      start = strstr( head, ",");
      if(start != NULL)
         start = start +1;
      else
         return;

      //first Message
      if(*(start+2)=='1')
      {
         memset( &g_svInfo, NULL, sizeof(g_svInfo));
         g_fgSVUpdate = false;
      }

      // Last Message
      if(*start == *(start+2))
         g_fgSVUpdate = true;

      //base
      start = strstr( start, ",");
      if(start != NULL)
         start = start +1;
      else
         return;
      if(!FetchField( start, result))
         return;
      base = (atoi(result)-1)*4;

      //total
      start = strstr( start, ",");
      if(start != NULL)
         start = start +1;
      else
         return;
      if(!FetchField( start, result))
         return;
      g_gpsInfo.SV_cnt = atoi(result);
      if(g_gpsInfo.SV_cnt == 0)
      {
         return;
      }

      for( i=0 ; i<4 ; i++)
      {
         //SVid
         start = strstr( start, ",");
         if(start != NULL)
            start = start +1;
         else
            return;
         FetchField( start, result);
         if(strlen(result)>0)
            g_svInfo[base+sv_cnt].SVid = atoi(result);
         else
            g_svInfo[base+sv_cnt].SVid = 0;

         //elev
         start = strstr( start, ",");
         if(start != NULL)
            start = start +1;
         else
            return;
         FetchField( start, result);
         if(strlen(result)>0)
            g_svInfo[base+sv_cnt].elv = atoi(result);
         else
            g_svInfo[base+sv_cnt].elv = 0;

         //azimuth
         start = strstr( start, ",");
         if(start != NULL)
            start = start +1;
         else
            return;
         FetchField( start, result);
         if(strlen(result)>0)
            g_svInfo[base+sv_cnt].azimuth = atoi(result);
         else
            g_svInfo[base+sv_cnt].azimuth = 0;

         //SNR
         start = strstr( start, ",");
         if(start != NULL)
            start = start +1;
         else
            return;
         if(*start == '*')
            g_svInfo[base+sv_cnt].SNR = 0;
         else
         {
            FetchField( start, result);
            if(strlen(result)>0)
               g_svInfo[base+sv_cnt].SNR = atoi(result);
            else
               g_svInfo[base+sv_cnt].SNR = 0;
         }

         sv_cnt++;

         if(base+sv_cnt == g_gpsInfo.SV_cnt)
            break;
      }
   }
}
//---------------------------------------------------------------------------
void GGA_Parse( char *head)
{
   //$GPGGA,144437.000,2446.367638,N,12101.356226,E,1,9,0.95,155.696,M,15.057,M,,*58
   char *start, result[20], tmp[20], *point;
   int len=0;

   // check checksum
   if(CheckSum(head, strlen(head)))
   {
      // UTC time : 144437.000
      start = strstr( head, ",");
      if(start != NULL)
         start = start +1;
      else
         return;

      if(FetchField( start, result))
      {
         // Hour
         strncpy( tmp, result, 2);
         tmp[2]='\0';
         g_gpsInfo.hour = atoi(tmp);
         // Min
         strncpy( tmp, result+2, 2);
         tmp[2]='\0';
         g_gpsInfo.min = atoi(tmp);
         // Sec
         strncpy( tmp, result+4, strlen(result)-4);
         tmp[strlen(result)-4]='\0';
         g_gpsInfo.sec = float(atof(tmp));
      }

      // Position(Lat)
      start = strstr( start, ",");
      if(start != NULL)
         start = start +1;
      else
         return;
      if(FetchField( start, result))
      {
         point = strstr( result, ".");
         len = (point-2)-result;
         strncpy(tmp, result, len);
         tmp[len]='\0';
         g_gpsInfo.Lat = float(atoi(tmp));
         strncpy(tmp, result+len, strlen(result)-len);
         tmp[strlen(result)-len]='\0';
         g_gpsInfo.Lat += float(atof(tmp)/60.0);
      }

      // N or S
      start = strstr( start, ",");
      if(start != NULL)
         start = start +1;
      else
         return;
      if(FetchField( start, result))
      {
         if(*result=='S')
            g_gpsInfo.Lat = -g_gpsInfo.Lat;
      }

      // Position(Lon)
      start = strstr( start, ",");
      if(start != NULL)
         start = start +1;
      else
         return;
      if(FetchField( start, result))
      {
         point = strstr( result, ".");
         len = (point-2)-result;
         strncpy(tmp, result, len);
         tmp[len]='\0';
         g_gpsInfo.Lon = float(atoi(tmp));
         strncpy(tmp, result+len, strlen(result)-len);
         tmp[strlen(result)-len]='\0';
         g_gpsInfo.Lon += float(atof(tmp)/60.0);
      }

      // E or W
      start = strstr( start, ",");
      if(start != NULL)
         start = start +1;
      else
         return;
      if(FetchField( start, result))
      {
         if(*result=='W')
            g_gpsInfo.Lon = -g_gpsInfo.Lon;
      }

      //GPS Fix Type and Service
      // 0: NoFix, 1:SPS, 2:DGPS, 6:Estimate
      start = strstr( start, ",");
      if(start != NULL)
         start = start +1;
      else
         return;
      if(!FetchField( start, result))
         return;
      g_gpsInfo.FixService = atoi(result);

      // Fix
      if(g_gpsInfo.FixService > 0)
      {
         if(g_gpsInfo.FixType == 0)
            g_gpsInfo.FixType = 1; // Assume 2D
      }

      start = strstr( start, ",");   // Number of SV in use , ex :9
      if(start != NULL)
         start = start +1;
      else
         return;

      // HDOP
      start = strstr( start, ",");
      if(start != NULL)
         start = start +1;
      else
         return;
      if(FetchField( start, result))
      {
         g_gpsInfo.HDOP = float(atof(result));
      }

      //Altitude (mean sea level)
      start = strstr( start, ",");
      if(start != NULL)
         start = start +1;
      else
         return;
      if(FetchField( start, result))
      {
         g_gpsInfo.Alt = float(atof(result));
      }

      //Altitude unit (bypass)
      start = strstr( start, ",");
      if(start != NULL)
         start = start +1;
      else
         return;

      //Altitude (Geoidal separation)
      start = strstr( start, ",");
      if(start != NULL)
         start = start +1;
      else
         return;
      
      if(FetchField( start, result))
      {
         g_gpsInfo.Alt += float(atof(result));
      }   
   }
}
//---------------------------------------------------------------------------
void __fastcall ZDA_Parse( char *head)
{
   // $GPZDA,000007.123,06,01,2000,,*50
   char *start, result[20], tmp[20];

   // check checksum
   if(CheckSum(head, strlen(head)))
   {
      // UTC time : 000007.123
      start = strstr( head, ",");
      if(start != NULL)
         start = start +1;
      else
         return;

      if(FetchField( start, result))
      {
         // Hour
         strncpy( tmp, result, 2);
         tmp[2]='\0';
         g_gpsInfo.hour = atoi(tmp);

         // Min
         strncpy( tmp, result+2, 2);
         tmp[2]='\0';
         g_gpsInfo.min = atoi(tmp);

         // Sec
         strncpy( tmp, result+4, strlen(result)-4);
         tmp[strlen(result)-4]='\0';
         g_gpsInfo.sec = float(atof(tmp));
      }

      // Day
      start = strstr( start, ",");
      if(start != NULL)
         start = start +1;
      else
         return;
      if(FetchField( start, result))
         g_gpsInfo.day = atoi(result);

      // Month
      start = strstr( start, ",");
      if(start != NULL)
         start = start +1;
      else
         return;
      if(FetchField( start, result))
         g_gpsInfo.mon = atoi(result);

      // Year
      start = strstr( start, ",");
      if(start != NULL)
         start = start +1;
      else
         return;
      if(FetchField( start, result))
         g_gpsInfo.year = atoi(result);
   }
}

//tear open sentence by "\r\n"---------------------------------------------------
void NMEA_Parse(void)
{
	char *GP;
	char type[4];

	memset(TempBuf,0,sizeof(char)*2048);
	strcpy(TempBuf,g_NMEAbuf);
	GP=strtok(TempBuf, "\r\n");

	if(GP == NULL)
		return;

   do
   {
      // Channel Status
      if(strncmp(GP, "$PMTKCHN", 8) == 0)
      {
         Channel_Parse(GP);
      }
      //Ack Parse
      else if(strncmp(GP, "$PMTK", 5) == 0)
      {
         Ack_Parse(GP);
      }
      // NMEA Parse
      else if((strncmp(GP, "$GP", 3) == 0) && (strlen(GP) > 10))
      {
         // skip "$GP" char to fetch Message Type
         strncpy ( type, GP+3, 3);
         type[3]='\0';

         if(strcmp( type, "GLL")==0)
         {
            GLL_Parse( GP);
         }
         else if(strcmp( type, "RMC")==0)
         {
            RMC_Parse( GP);
         }
         else if(strcmp( type, "VTG")==0)
         {
            VTG_Parse( GP);
         }
         else if(strcmp( type, "GSA")==0)
         {
            GSA_Parse( GP);
         }
         else if(strcmp( type, "GSV")==0)
         {
			GSV_Parse( GP);
         }
         else if(strcmp( type, "GGA")==0)
         {
            GGA_Parse( GP);
         }
         else if(strcmp( type, "ZDA")==0)
         {
            ZDA_Parse( GP);
         }
      }
   }while( (GP = strtok( NULL, "\r\n")) != NULL );
}

void FixSVMapping(void)
{
   int i, j;

   //Clear
   for(i=0 ; i<NUM_CH ; i++)
   {
      g_svInfo[i].Fix = 0;
   }

   for(i=0 ; i<g_gpsInfo.SV_cnt ; i++)
   {
      j=0;
      while(g_gpsInfo.fixSV[j] > 0)
      {
         if(g_svInfo[i].SVid == g_gpsInfo.fixSV[j])
         {
            g_svInfo[i].Fix = 1;
            break;
         }
         j++;
      }
   }
}

int SVidSort( const void *a, const void *b)
{
   SVInfo *sv1, *sv2;

   sv1 = (SVInfo *)a;
   sv2 = (SVInfo *)b;

   if(sv1->SVid >= sv2->SVid)
      return 1;
   else
      return -1;
}

void GenerateCkSum(char *buf, char *result)
{
   char chksum;
   int i,size;

   chksum = buf[1];
   size = strlen(buf);

   for(i=2; i<size ; i++)
      chksum ^= buf[i];

   sprintf(result, "%02X", chksum);
}
bool GenerateFullCmd(char *buf, UINT len)
{
   char CkSum[3];

   ASSERT( strlen(buf) < len-5 );

   if ( strlen(buf) < len-5)
   {
	  GenerateCkSum(buf,CkSum);
	  strcat(buf,"*");
	  strcat(buf,CkSum);
      strcat(buf,"\r\n");
	  return true;
   }
   else
   {
	  return false;
   }
}
